function [noisydata] = fcn_ext_v_p(vel, pos, sigma_vel, sigma_pos, dt, dt_ext_pos)
%fcn_ext_position 	This function generates external position and velocity
%   [noisydata] = fcn_ext_v_p(vel, pos, sigma_vel, sigma_pos, dt, dt_ext_pos)

%   This function generates a noisy external position and vlocity for 
%   simulation. A simple Gaussian White Noise is assumed.

%   IN      vel         Velocity vector (inertial frame)
%   IN      pos         Position vector
%   IN      sigma_vel   Standard deviation of velocity [m/s]
%   IN      sigma_pos   Standard deviation of position [m]
%   IN      dt          Sample period of IMU
%   IN      dt_ext_pos 	Sample period of external position (usually <=10Hz)       
%   OUT     noisydata   Noisy position data

%% Mag errors
n = length(pos);
if (~isvector(pos))
    b = size(pos,2);
else
    b = 1;
end
noisydata.vel = zeros(n, b);
noisydata.pos = zeros(n, b);

% Nv and Np are zero-mean random variables with unit variance
unit_variance = 1;
std = sqrt(unit_variance);
Nv = std * randn(n, b);
Np = std * randn(n, b);

noisydata.vel = vel + sigma_vel * Nv;
noisydata.pos = pos + sigma_pos * Np;

% Remove data between sampling points
time_ext_pos = dt_ext_pos - dt;
for j=1:n,j;
    
    time_ext_pos = time_ext_pos - dt;
   
    if (time_ext_pos <= 0)
        time_ext_pos = dt_ext_pos - dt;
        noisydata.vel(j,:) = noisydata.vel(j,:);
        noisydata.pos(j,:) = noisydata.pos(j,:);
    else
        noisydata.vel(j,:) = 0;
        noisydata.pos(j,:) = 0;
    end
    
end

end

